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ABSTRACT 

This paper presents development of a generic recursive 
Order-N algorithm for systems with rigid and flexible 
bodies, in tree or closed-loop topology, with N being the 
number of bodies of the system. Simulation results are 
presented for several test cases to verify and evaluate the 
performance of the code compared to an existing efficient 
dense mass matrix-based code. The comparison brought 
out situations where Order-N or mass matrix-based 
algorithms could be useful. 

INTRODUCTION 

The Software, Robotics and Simulation Division (SRSD) 
of NASA Lyndon B. Johnson Space Center provides math 
modeling and simulation in support of engineering 
analyses and crew training activities for the center. The 
division currently has an efficient generic multibody 
dynamics code based on a dense mass-matrix formulation, 
which is used for simulating systems involving on-orbit 
robotic manipulators such as the Canadian Space Agency- 
built Space Station Manipulator System (SSRMS). It is 
generally known that Order-N (O(N)) algorithms, which 
involve arithmetic operation counts of the order N, where 
N is the number of bodies, perform more efficiently for 
systems with large degrees of freedom, compared to mass 
matrix-based with operations of order N 3 . It was therefore 
decided to develop an O(N) simulation for SRSD to 
investigate applications where they may perform better. 
This development was performed in-house to allow 
maximum flexibility and control in different simulations. 

ALGORITHM DEVELOPMENT 

There are several methods in the literature that may be 
used for developing an O(N) algorithm. References may 
be found in (Banerjee, 2003). The formulation presented 
here is based on algebraically putting together the 
following steps: (1) kinematic equations relating motions 
between consecutive joints, (2) equations of motion of a 
single body, rigid or flexible, (3) equations relating the 
total spatial force and active forces and moments at the 
joint and (4) constraint conditions. 
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Derivation of the Order-N algorithm is presented in the 
Appendix. The effect of motion-induced stiffness 
(Banerjee, 1993) in flexible bodies of the system has not 
been incorporated yet. Inter-body forces may cause this 
effect to be important in some cases even for slow 
motions typical of space systems. 

CODE VERIFICATION 

The Order-N code was verified against the existing mass 
matrix-based code for several test cases, which itself was 
verified against other simulations in the industry, 
including TREETOPS (Singh et. al., 1985). The mass- 
matrix algorithm has been in use for many facilities at the 
Johnson Space Center for many years. Results from the 
two implementations matched with high accuracy (i.e., 
within 1.0e-10 or better). 

SIMULATION TEST CASES 


The test cases are based on variations of the system 
shown in Figure 1. The base plate B 0 is a rigid circular 
plate floating in inertial space. 



Figures 1: Test Model Description 

Three identical articulating legs and two identical 
manipulator arms are rigidly attached to the top plate. 
The other ends of the legs are rigidly connected to the 
base plate and the other ends of the manipulators hold a 
test object rigidly. All links are modeled as cylindrical 
rods. All joints of the legs and manipulators are single 
axis rotational joints. Two configurations of the legs and 
manipulators are considered, one with six joints the other 
with seven. The axes of the seven jointed manipulators 
and legs are in the order roll, yaw, pitch, pitch, pitch, yaw 



and roll. The axes for ones with six joints are in the order 
roll, yaw, pitch, pitch, yaw and roll. In several 
configurations the boom elements of the legs have been 
split into two parts of equal length and joined rigidly, for 
adding additional bodies and flex degrees of freedom to 
the system. Only the boom elements are modeled as 
flexible. Each flexible rod has four bending modes (two in 
each of the bending planes). The system was driven by 
forces and moments on the top plate and the test object, 
and moments on joints of the first leg. The forces and 
moments were held constant for every 20 seconds and 
then switched in sign. Table 1 shows the configurations. 
Open loop cases are obtained by freeing the joints 
between legs 2, 3 and top plate and between manipulator 
2 and the test object. For closed loop simulations 
constraint forces were determined at these same points. 


Table 1 : System Test Case Configurations 


System Configuration 

Legs 

Manipulators 

Total Number of Bodies 

Number of 
Bodies 

Number of 
Joints 

Boom 

Construction 

Number of 
bodies 

Number of 
Joints 

1 

6 

6 

One rod 

0 

0 

20 

2 

8 

6 

Two rods 

0 

0 

26 

3 

10 

7 

Two rods 

0 

0 

32 

4 

10 

7 

Two rods 

8 

7 

49 
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Figures 2: Displacements of Top Plate and Test Object 


Plots of representative data are shown in Figures 2 and 3. 
Figure 2 shows a co-plot of displacements of the center 
of the top plate, and the joint between manipulator 1 and 
the test object obtained from O(N) and mass matrix 
simulations for Case 16. The two results match to within 
1.0e-10. Figure 3 is a co-plot of angular acceleration of 
the bottom pitch joint of leg 2 for Cases 12 and 16 (rigid 
and flex respectively) for 0(N). Distances are shown in 
meters, while angles are shown in radians. 
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Figures 3: Leg 2 Bottom Pitch Joint Angular Acceleration 

Performance of the O(N) code is measured by the CPU 
time for a 100 second simulation and comparing it with 
the existing code. The results of the comparison are listed 
below for rigid and flexible models separately for both 
open-loop and closed-loop scenarios. Rigid cases were 
run with 0.001 second and flexible cases were run with 
0.0001 second integration time step. An Euler-Cromer 
integration scheme was used. 


Table 2: Timing Results 


Case Number 

Rigid (R) or Flex (F) Simulation 

Configuration (Table 1) 

Open (O) or Closed(C) loop 

Degrees of 
Freedom 

CPU Time 
(sec) 

Rigid 

Flex 

Total 

Order-N 

Mass 

Matrix 

1 

R 

1 

O 

24 

0 

24 

10.1 

6.7 

2 

R 

2 

O 

24 

0 

24 

12.4 

7.9 

3 

R 

3 

O 

27 

0 

27 

15.0 

10.1 

4 

R 

4 

O 

42 

0 

42 

22.3 

18.6 


5 

F 

1 

O 

24 

24 

48 

166.8 

174.0 

6 

F 

2 

O 

24 

48 

72 

277.9 

423.6 

7 

F 

3 

O 

27 

48 

75 

275.9 

478.0 

8 

F 

4 

O 

42 

80 

112 

408.6 

1127.9 

9 

R 

1 

C 

24 

0 

24 

40.7 

17.1 

10 

R 

2 

C 

24 

0 

24 

48.0 

18.4 

11 

R 

3 

C 

27 

0 

27 

59.4 

22.7 

12 

R 

4 

C 

42 

0 

42 

133.8 

57.4 

13 

F 

1 

C 

24 

24 

48 

613.4 

567.6 

14 

F 

2 

C 

24 

48 

72 

937.2 

1402.0 

15 

F 

3 

C 

27 

48 

75 

1037.9 

1551.2 

16 

F 

4 

c 

42 

80 

112 

2257.3 

4063.9 


Cases 9, 10, 11, 13, 14 and 15 have two loops, cases 12 
and 16 have three loops. 

DISCUSSION AND CONCLUSIONS 

The timing results confirm that the dense mass matrix 
formulation is faster than the O(N) formulation for 
smaller degrees of freedom (DOF’s) and O(N) is faster 
otherwise. In our cases for all rigid systems mass-matrix 
take less CPU time than 0(N) for both open-loop and 
closed-loop systems because they have fewer DOFs. 
Flexible body systems have more DOF and 0(N) run 
faster, except for the case 13 involving two loops and 
fewer DOF than the other flex cases. Loops added large 
amount of computation and more so for O(N). For 
systems with loops the advantage of 0(N) is reduced. 
With more loops the reduction is more because it requires 
solution of coupled linear constraint equations which can 
have a large number of unknown constraint forces and 
moments, and are solved in the usual manner that requires 

n c arithmetic operations, where n c is the number of 
constrained degrees of freedom. 

The high degree of match between the mass-matrix and 
0(N) results is expected because the two methods solve 
the same set of equations using many common codes. 


ACKNOWLEDGMENT 

The authors acknowledge the support provided by An 
Huynh and Thomas Brain of METECS Corporation, 
Houston Texas, USA in coding the simulation, and to 
Arun Banerjee, formerly of Lockheed Martin Palo Alto 
Research Laboratory, California, USA for review of the 
manuscript. 


REFERENCES 

Banerjee, A. K, “Block-Diagonal Equations for 
Multibody Elastodynamics with Geometric 
Stiffness and Constraints”, Journal of Guidance, 
Control and Dynamics , Vol. 16, No. 6, 1993. 
Banerjee, A. K., “Contributions of Multibody 
Dynamics to Space Flight: A Brief Review”, 
Journal of Guidance, Control and Dynamics , Vol. 
26, No. 3, 2003. 

Quicho, L,J.; 2010, T. Ghosh, D. frenkel, A. Huynh, 


“Mode Selection Techniques in Variable Mass 
Flexible Modeling”, In AIAA Meeting Papers 
AIAA Modeling and Simulation Technologies 
Conference, Aug 2010, Toronto, Canada, AIAA- 
2010-7606. 

Singh, R.P., van der Voort, R.J., and Likins, P.W., 
“Dynamics of Flexible Bodies in Tree Topology-A 
Computer Oriented Approach”, Journal of 
Guidance, Control and Dynamics , Vol. 26, No. 3, 
1985. 


APPENDIX: DERIVATION OF O(N) EQUATIONS 


System Description and Definitions 



Figures A1 : Multibody System Definitions 



Figures A2: Constrained Motion Definitions 

Consider the set of rigid and/or flexible bodies, in Figure 
Al, with the associated labeling of forces and points. 
9l n and Sir are frames attached to body n (B n ) at its 

inboard and outboard joints O n and respectively. 

Figure A2 shows the labeling for constrained systems. 
Throughout this derivation it is assumed that all the 
vectors and inertia matrices are either generated in or are 
converted to a single reference frame, which is the 
structural reference frame SIq of the base body of the 
multibody system. The conversion is performed every 
time the system states are updated. 

Kinematics of Motion between O p and O n 

Let v n , a n represent the inertial velocity and acceleration 
respectively, of O n , co n represent the inertial angular 


(1.4) 


velocity of 9^ and r| p represent the flexible variables of 
the body B p . Kinematic equations relating position, 
velocity and acceleration of O p and , and angular 
velocity and angular acceleration of frames Sip and 
Sl^ are given by 

f n=Pn +( l ) n T lp> v n= V p +© p r n +<|> fi r| p , 

ff>n = 6) p + 'KnTlp 

ro fi =K.p+\)/ fl rin+ co p v|/ fi ii n 

afi =a p - r n ro p +<^ il r\ p + ro p ro p r n +2S p 4> fi ri p 
p p is the undeformed value of r p , the vector fromO p to 
• Combining the last two equations we can write 

^■n "*"^n,r (1*1) 


where, A^= 



1 

0 1 


> s n 


’t'n 

Vn 


.ndA 4 l .|W + 25,*inp| 

’ [ ^pVCMlPn J 

$ denotes a shift operator corresponding to an offset 
between two points identified by the subscripts. In this 
case the offset is r^ . The tilde (~) is the usual cross 
product operator on a vector. 


The joint n may have up to six DOFs allowing both 
translation and rotation. The translation of O n with 

NT n 

respect to is given by y n T§iA» =G . 5 » Where 

i=l 

gj n are linearly independent unit vectors fixed in , 
NT n < 3 is the number of translational degrees of 
freedom of joint n and 8j n are scalar quantities, 

representing joint translations in the directions of the unit 
vectors. The inertial acceleration of O n is: 

a„=afi-yn®A+G„8 n +a7 (1-2) 

where, = 5 A 5 A y n +25 A G n 8 n 


Inertial angular velocity of 9^ is given by 
NR Da , 

^ li,n^i,n — C0^+L n 9 n 
i=l 

where n is the unit vector in the direction of the 
rotation 9j n and NR n is the number of rotational degrees 
of freedom of the joint. L n is a matrix whose columns are 
the unit vectors lj n and 0 n is a column matrix containing 
the rotations 0j n . Differentiating co n in the inertial frame 
we get 

co n- co n +L n^n +a n (1*3) 

rv r ® — T A 
a n ^lWn 

Combining Equations (1.2) and (1.3), 


A n - ^n,n A n 


P nYn 


A„=< 

A 

,®n. 


_ fsj 
le n J ’ 

An = 

K 

®n,n = 

"1 

0 

-y n " 

i 

and P n = 

"G„ 

0 

0 " 
L n_ 


(1.5) 

Finally, combining Equations (1.1) and (1.4) we get 

A n ~^n,p A p + + P nYn A n (1-^) 

where, using the relationship r n = % + y n , 

S a ,p= S nAp> S n=\nS„> 


Inter-Body and Joint Actuator Forces 

Let F n be the spatial forces exerted by B p on B n at 
O n and F^ be the spatial force exerted by B n on B p at 
. Then from equilibrium considerations, 

F fl =-g T .F n (2.1) 

n n,n n v y 

ef n £is given by Equation (1.5). Let (iq n and v j n be 
respectively, the i-th actuator force (i<3) and j-th actuator 
torque (j<3) on body n at O n . They act in the directions 

of the degrees of freedom g i?n and lj n of the joint. 


Defining a n = 


Bn 

v n 


as the array of actuator forces and 


moments at joint n, it is straight forward to show that 

a n = P n T F n (2.2) 

where P n is given by Equation (1.5). 


Equations of Motion of Body n 

A minor modification of equations of a flexible body in 
Quiocho, et. al (Quiocho, 2010), produces the equations 
of a body B n in rigid and flexible coordinates as 

M rr,n A n +M re,nTn =X^U F e,i,n +B r,n 
i 

■^■er,n A n ~*”^ee,n4n ~*”^ee,n4n ”*”Dee,nTn 

= yAn F e,i,n + B e,n 
i 

We shall write the equations of motion of the body n after 
separating the external spatial forces F e i n on B n as (i) 

from the previous body acting at the inboard joint (F n ), 

(ii) from child bodies B k . acting at outboard joints ( Fr ), 

1 H 

(iii) forces at constrained points (F c j) when there are 

such points on the body, and (iv) external forces ( F ext,i,n ) 

acting on the body. Because we chose the reference frame 
of the body to be at the inboard joint, the shift function at 
the inboard joint is an identity matrix and the shape/slope 
function at the inboard joint is a null matrix. Equations for 
body n are: 



M rr,n A n +M re,n4n- F n + X^ T n ^q + 

X*J F cJ + X^i-ri F ext-i-ri "*"®r,n 

jeZ c (n) i 

^er,n A n "*"-^ee,n^in “*"^ee,n c ln “l~ B ee,nTn — XX F f. 

i 1 1 

+ X S c,j F c,j + X S f n F ext,i,n + B e,n 
j^Z c (n) i 

In the above equations F c j is the spatial force at the 

j-th constrained point, $ c j is the shift function for the 

offset of the constrained point from O n and S c j is the 

shape/slope function of body n at the constrained point. 
Defining 


G r,n =Z^i T n F ext,i,n + B r,n 


( 3 - 1 ) 


G e,n =X S ^ Fex ‘.i. n + B e,n _K ee,nq n - D ee,n 4 n ( 3 -2) 


Using Equations (2.1) for F- and then Equations (3.1) 
and (3.2) we get the equations of motion of the body n as 

M n-,n A n + M re,nfin = F n “X^A- n 

(3.3) 


•,n A n + M re,nfin “ F n _ XX nFk i 

X^c,j F c j +G r?n 
jeZ c (n) 


A ^er,n A n Mee,nfin X^k^ F kj X^c,j F c,j G e,n 
i jeZ c (n) 

(3.4) 

Here, Z c (n) is the set of indices for constrained points on 
body n. 


Recursive Solution of Equations of Motion 

The dynamical equations of motion of the system are 
solved recursively in steps as follows. 

Step 1 . Forward Pass Kinematics: In this step all position 
and velocity states are generated starting with the base 
body using equations derived in Kinematics section. 

Step 2. Inward Pass Dynamics Equations: 

Observing the equations of motion it is reasonable to 
expect that for any n F n can be expressed as linear 
functions of A p , q p and constraint forces on itself and on 
bodies on outer branches: 

F n = D A,n A p + D q,nfip + d n + X D c,n,j F c,j C 4 - 1 ) 

j^Y c (n) 

Y c (n) is the set of indices of all constrained points on 
body n and on all bodies of its outer branches. 

We shall determine the coefficients in the above equations 
recursively. In Euation (4.1) using Iq in place of n and n 
in place of p, and using the result in Equation (4.1) we get 

q„ =Q a ,„ a „ +4„ + X < Tnj F cj ( 4 - 2 ) 

jeY c (n) 


QA,n M ee?n M er?n , M ee?n -^ee,n + X^kj B q,ki 

ieZ(n) 

M er ,n = M er,n + Z^k^A,^ 

ieZ(n) 

q„=M; e 1 ,„[Ge,n- 2X d k[ ] 

ieZ(n) 

Qc,n,j = -M e e,n S ki D c,k, J for i e Z(n), j e Y c (k ; ) 

Qc,n,j = -M ee ,n S c,j for j eZ c( n ) 

Z(n) is the set of indices of child bodies of B n . In the 
same manner, substituting for F k . in Eq. (3.3) and 
rearranging we get, 

M rr,n A n + M re,nfin = F n + G r,n + X! D c,nj F c,i ( 4 - 3 ) 

jeY c (n) 

M rr,n = M rr,n + X ^ ,n DA ’ k i ’ 
iEZ(n) 

M re,n = M re,n + X^q,n D qTi ’ 
ieZ(n) 

G r,n = G r,n “ X^q,n dk i ' 
iEZ(n) 

D c,n,j =-^,nD c ,kiJ for ieZ ( n )> j e Y c (kj) 

D Cj n,j ^,j fo^* j ^ Z c(rr) 

Using Equation (4.2) for q n in Equation (4.3) we get 
Mrr, n A n =F n +G pn + X B c,n,j F c,j ( 4 - 4 ) 

jeY c (n) 

Mrr,n = Mrr, n Mre,nQA,n , G r,n = G r,n — Mre,nfin 
B c,n,j — B c,n,j — ^re^QcAid 

Using Equation (1.6) for A n in Equation (4.4) and 
rearranging, we get 

^n-,A,p A p + Mn^ n S n;P q p +M rrn Pny n 

= F n + G r,n — ^ 4 rr,n A n,R + X^c,n,j F c,j 
j^Y c (n) 

T 

Pre-multiplying this equation byP n , using 
a n = P n F n from Equation (2.2) and solving the resulting 


equation for y n we get 


Yn = 

B A,n A p +B q,nfip + F n ' 

f x^ c ’ n ’j F c,j 



jeY c (n) 

B A,n 

— “ Myy_ >n P n , 


B q,n 

= — M^nPn M rr , n S n;P , 


II 

Cl 

- M ik +P » T A- 

■M^nA^R)], 


B c,n,j Myy,n F n D c ,n,j , and Myy ?n P n M rr ^ n P n 

Equation (4.5) for y n in Equation (1.6) yields for A n 

A n — ^A,n A p + ^q,nfip w n X^c,n,j F c,j 

jeY c (n) 


(4.6) 



W A,n - Ap + P n B A,n > W q,n “ S n,p + P n B q,n 

w n = P n b n + A n>R , and W C;I1 j = P n B c nJ 


Equation (4.6) for A n in Equation (4.2) gives 

4n = QA,n A p + Qq,n4p + 4n + XQc,n,j F c,j ( 4 - 7 ) 

jeY c (n) 

QA,n = QA,n^A,n » Qq,n — Qn^q,n » 4n — QA,n w n 4 n ? 

and Q c?n j = QA,n^c,n,j Qc,n,j 


Finally, using Equation (4.6) in Equation (4.4) we get 

F n = D A,n A p + D q,n4p +4 n + X D c,n,j F c,j ( 4 - 8 ) 


j^Y c (n) 


rr,n vv A,n > D q,n = M rr,n W q,n 


D A ,n=M rrTl W < 

4 n= M rr,nW n -G r?n ,and 


D =M W -D 

lv± rr,n vv c,n,j c,n,j 


We can see that Equation (4.8) is in the same form as 
Equation (4.1) we started with, confirming that if the 
latter is true for child bodies, it would be true for the 
current body also. Following the same steps as above for 
any outermost body for which F^ = 0 it is easy to show 

that Equation (5.1) is true for such bodies. Using the 
induction logic it therefore follows that Equation (4.1) is 
true for all bodies of the system. 

Computations for systems with multiple branches need to 
start from an outermost body, move inward till a body 
with multiple child bodies is reached. Inward computation 
from a body with multiple child bodies should continue 
only after computations for all of its child bodies are 
completed. 

A 0 and F 0 for the Base Body B 0 in terms of the 
constraint forces are obtained from Equation (4.4). When 
B 0 is fixed in inertial frame A 0 = 0 and we then have 

F 0 = “G r?0 + X^c,0.j F c,j 
j G Y c (0) 

When B 0 is free in the inertial frame F 0 = 0 and 

Ao=M-| 0 [G r; o+ ZAo.jFc.j] (4.9) 

j G Y c (0) 

The flexible coordinate acceleration q 0 for the base body 
is obtained from Equation (4.2) 

40 = Qa, 0 A 0 + 4o + XQc,n,j F c,j (4.10) 

j G Y c (n) 

For systems without closed loops the F c jterm drops out 

and the equations obtained above are sufficient for 
determining A 0 and q 0 , and then the system 
accelerations, by successive computation of y n , q n and 
A n in an outward sweep. 

Step 3. Accelerations of Constrained Points in terms of 
Forces at Constrained Points: 


For the determination of acceleration of constrained 
points in terms of forces at these points we seek to 
express the accelerations A n and q n for bodies in the path 
from base body to the constrained points in the form 

A n =h£ + 2XjF c>j and 4n=h2+Z H n,jAj 

(4.11) 

Here the summation index j covers all constrained points. 
It follows from Equations (4.9) and (4.11) that when the 
base is free in inertial frame 

h^ = M^qGj- q , Hqj = M^oD^j ? h$ = Qa,o F 0 ^ + 4o 
andHg =Q C;0;j . 

When the base is fixed, hp 1 = 0 , Hqj = 0 , hjj = q (J and 

H 0=<W 

Using Equations (4.7) and (4.11) we get the recursive 
equations for hjj and Hjj • : 

h n = QA,n h p + Qq,n h p + fin and 

H n.j = + Qq.nHpj + Q c ,n,j 

Using Equation (4.6) and (4.11) we get the recursive 

equations for h^ and H^j : 

hn = W A , n hp + w q n h^ + w n , and 
j = W A n H V + W qn H^j + W cn j . 

Let Ej be the point in constraint i located on body n and 
Ej, be the mating point of the same constraint, located on 
body i . The spatial acceleration of Ej is 
A Ei =% i ,n A n+SE i ,n4n +A E i ,r (4-12) 

where %. n is the standard shift operator for the offset 

r E . of Ejwith respect to O n S E . n = 

shape/slope function of B n at Ej and, 

_pn(S n f Ei + 2 <t , E i q n )’ 

Ei,r 1 ranVEjfin 


<t>E, 

VEj 


is the 


For each constraint k (k = 1, 2, ...,n c ) the constraint 
forces F ck and F ck . at the two constrained points E k 
and E k - respectively are equal and opposite, i.e, 
F c k . = -F c k . Using this and Equation (4.1 1) for A n and 
q n in Equation (4.12) we get, 

A Ej = g k i ,n ll n + S Ei,n h n + A E i? r + 


Z[%,„(HZ -HA) + S Ei , n (H^ k -H; >k .)]F c>k 

(4.13) 


k=l 


Similarly, spatial acceleration of Ej- is given by 

A E r = % it ,n F f + ^E it ^ F ? +A 


Ej.,r ' 


-HA) + SE,/(H^ k - H ^ k . )]F C 


k=l 



(4.14) 


Step 4. Determination of Constraint Forces: 


Using equations (4.13) and (4.14) the difference in the 
accelerations at the constraint i may be written as 

AA-E; =A Ej _a E ; . =A h E . + Z AH E i ,k F c,k (4.15) 

k 

Ah E . = §E.,/h^ + S E ., E h E + A E . i>r 

“^Ej.nhn “^Ej.nhn _A E;,r 

AH Ei>k =3fe. f>/ (H^k-H^kO-% i .n(Hi; k -H A k .) 

+ S Ei . ; ,(H^ k -H^ k ,)-S Ei;n (H^ k -H^ k ,) 

The summation range for k is all the constraints. 

The linear and angular constraints at are 
g c ,i,j • (vej, - v Ei ) = o for 0 < j < n t j 

Uj*(“e ; . -®Ei) = 0 for 0 < j < n r j 

where v and co represent the inertial velocity and angular 
velocity of points corresponding to the subscripts, 
g c? ij and f c ijare unit vectors in the direction of 

translational and rotational constraints respectively, and 
n t j(<3) and n ri (<3)are the number of these 

constraints, respectively. v E , v E , c6 E . and o5 E . are 

determined in the manner used for the point O- in the 
Kinematics section,. Defining 


AV f . = 


V E , -^Ei" 

and P c j = 

G c,i 

0 " 

®Eji “^Ei 


0 

hc,i 


where 


G c j is a 3 x n t j matrix whose columns are the unit 
vectors g c>i j and L c is a 3 x n r i matrix whose columns 

are the unit vectors i c ,i,j> the difference in the spatial 
velocities of the constrained points at constraint i may be 
written asP ( 7 i AV E . . The difference in the spatial 

acceleration in the constraint directions at the constraint 
point should be zero, giving 

P c T i AA Ei + Pc T iAV e . = 0 

Using Baumgarte’s stabilization scheme to limit 
constraint violation caused by numerical errors, this 
equation is modified to 

P c T iAA Ei +P c T iAV Ei +K i P c T i AX E . +C i P c T i AV Ei =0 
Kj and Cj are positive constants to provide constraint 
stabilization. Using Equation (4.15) for AA e . we have 

Pc T iZAH E . ;k F C;k =-P c T iAh Ei -P c T iAV E . 


(4.16) 

Let us define f c?k jto be the constraint force in the 


-K,pT AX E| -C,P c l ,,AV ti 


direction g C;k j and x C;k ,j the constraint torque in the 

f c,k 

T c,k_ 


direction f ck jand F c?k = 


. The force F c k at the 


constraint point E k due to forces and moments in the 
constraint directions is then given by P c k F c k . Let 


p f,k = 


where G f k and L f k are made of 


f,k 4 


'Gf, k 0 ‘ 

0 L f,k_ 

the unit vectors normal to the constraint directions for 
translation and rotation and Ff k be the array of forces and 
moments in these directions. Net spatial force at E k is 
F c,k = Pc,k F c,k + Pf ,k F f ,k (4-17) 

Restricting to cases where F f k is fully known, we use 
F c,k = PcAk + p f,k F f,k in Equation (4.17) to get 


P c T iZ AH E 1 ,kP c ,k F c,k=Z c ,i (4-18) 

k 

Zc,i=-Pc T i[E AH E..k P f.k F f,k +Ah E i ] 

k (4.19) 

-p c >v E _ -k,p c t ,ax Ei -c,p l t ,av E| 

Stacking Equation (4.18) for all constraints we get 


M C F C = Z c (4.20) 

where the [i, k] submatrix of matrix M c is given by 

h4 c ,i,k = Pc,i A ^E E kPc,k 

and Z c is made of arrays Z c ^ given by Equation (4. 19). 
Equation (4.20) is solved for F c and the spatial forces 
F c k at constraint points are found using Equation (4.17). 


Step 5. Computation of System Accelerations: 

After determination of forces at the constrained 
points, Aq and f| 0 are determined using Equations (4.9) 
and (4.10). y n , q n , and A n are determined recursively in 

a forward pass using Equations (4.5), (4.7) and (1.6) 
respectively. 




